#include "precom.h"
#include "ArcBall.h"

namespace {
	const float STANDARD_TRANSLATION = 10.f;
	const int IL_FRAGMENTOW_KOLA = 50;
	const float PROMIEN_PIERSCIENIA = 250.0f;
}

void ArcBall::init(int w, int h) 
{
	m_wndWidth = w;
	m_wndHeight = h;	
	m_mouseIntPos[0] = m_mouseIntPos[0] = 0;
	m_lastRot.identity();
	m_curRot.identity();
	m_isClicked = m_isRClicked = m_isDragging = false;
	m_isCursor = true;
	m_quat.set(0.0f, 0.0f, 0.0f, 0.0f);
	m_lastTrans.set(0.f, 0.f, -STANDARD_TRANSLATION);
	m_trans.set(0.f, 0.f, -STANDARD_TRANSLATION);
	m_mouseEnd.set(0.0f, 0.0f);
	m_mouseStart.set(0.0f, 0.0f);
	m_mouseZStart = m_mouseZEnd = 0.0f;
	m_transform = fmath::TranslateMatrix44(m_lastTrans.x, m_trans.y, m_lastTrans.z);
	m_scale = 1.0f;
	m_invert.identity();
	m_aInvert = true;
	m_centering.set(0.0, 0.0, 0.0);
}

ArcBall::ArcBall() 
{
	init(800, 600);
}

ArcBall::ArcBall(int a_width, int a_height)
{
	init(a_width, a_height);
}

fmath::vector2 ArcBall::ReturnMapMousePosition(int x, int y) {
	fmath::vector2 mousePos;
	int size = min(m_wndHeight, m_wndWidth);
	float div = static_cast<float>((size - 1) / 4);
	mousePos.x = (static_cast<float>(x - (m_wndWidth-1)/2) / div);
	mousePos.y = -(static_cast<float>(y - (m_wndHeight-1)/2) / div);
	return mousePos;
}

fmath::vector2 ArcBall::ReturnMapMousePosition(int x, int y, fmath::vector3& o_vec) {
	fmath::vector2 mousePos;
	int size = min(m_wndHeight, m_wndWidth);
	float div = static_cast<float>((size - 1) / 4);
	mousePos.x = (static_cast<float>(x - (m_wndWidth-1)/2) / div);
	mousePos.y = -(static_cast<float>(y - (m_wndHeight-1)/2) / div);
	float len = mousePos.lengthSqr();
	if(len > 1.0f) {
		mousePos /= sqrtf(len);
		o_vec.x = mousePos.x;
		o_vec.y = mousePos.y;
		o_vec.z = 0.0f;
	} else {
		o_vec.x = mousePos.x;
		o_vec.y = mousePos.y;
		o_vec.z = sqrtf(1.0f - len);
	}
	return mousePos;
}

void ArcBall::MousePosition(int x, int y) {
	
	m_mouseEnd = ReturnMapMousePosition(x,y,m_endVec);
	fmath::vector3 prepen = fmath::CrossProduct(m_startVec, m_endVec);
	if(prepen.length() > EPS) {
		m_quat.vec.x = prepen.x;
		m_quat.vec.y = prepen.y;
		m_quat.vec.z = prepen.z;
		m_quat.w = fmath::DotProduct(m_startVec, m_endVec);
	} else {
		m_quat.set(0.0f, 0.0f, 0.0f, 0.0f);
	}
}

void ArcBall::SimulatedWheel(int deg)
{
	m_mouseIntPos[2] -= deg;
	m_isRClicked = true;
}

void ArcBall::MouseWheel(WPARAM wParam)
{
	m_mouseIntPos[2] -= GET_WHEEL_DELTA_WPARAM(wParam)/30;
	m_isClicked = (LOWORD(wParam) & MK_LBUTTON) ? true : false;
	m_isRClicked = (LOWORD(wParam) & MK_RBUTTON) ? true : false;
}

void ArcBall::MouseMove(LPARAM lParam, WPARAM wParam)
{
	m_mouseIntPos[0] = LOWORD(lParam);
	m_mouseIntPos[1] = HIWORD(lParam);
	m_isClicked = (LOWORD(wParam) & MK_LBUTTON) ? true : false;
	m_isRClicked = (LOWORD(wParam) & MK_RBUTTON) ? true : false;
}

void ArcBall::TurnOnOrtho2D(int w, int h)
{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D(0, w, 0, h);
	glTranslatef(w/2, h/2, 0.0);
	glMatrixMode(GL_MODELVIEW);
}
void ArcBall::TurnOnPerspective(int w, int h)
{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(45.0f, (GLfloat) w/(GLfloat) h, 1.0f, 100.0f);
	glMatrixMode(GL_MODELVIEW);
}

void ArcBall::createMatrix44(fmath::matrix44& m4, 
									 const fmath::matrix33& m3,
									 const fmath::vector3& v3) const
{
	m4.col[0][0] = m3.col[0][0];
	m4.col[0][1] = m3.col[0][1];
	m4.col[0][2] = m3.col[0][2];

	m4.col[1][0] = m3.col[1][0];
	m4.col[1][1] = m3.col[1][1];
	m4.col[1][2] = m3.col[1][2];

	m4.col[2][0] = m3.col[2][0];
	m4.col[2][1] = m3.col[2][1];
	m4.col[2][2] = m3.col[2][2];

	m4.col[3].x = m_centering.x * m3.col[0][0] 
		+ m_centering.y * m3.col[1][0] 
		+ m_centering.z * m3.col[2][0] + v3.x;
	m4.col[3].y = m_centering.x * m3.col[0][1] 
		+ m_centering.y * m3.col[1][1]
		+ m_centering.z * m3.col[2][1] + v3.y;
	m4.col[3].z = m_centering.x * m3.col[0][2] 
		+ m_centering.y * m3.col[1][2]
		+ m_centering.z * m3.col[2][2] + v3.z;
}

void ArcBall::MouseUpdate()
{
	if(!m_isDragging) {
		if(m_isClicked) {
			m_isDragging = true;
			m_lastRot = m_curRot;
			ClickMouse(m_mouseIntPos[0], m_mouseIntPos[1]);
		} else if(m_isRClicked) {
			//rozpoczac przesuwanie
			m_isDragging = true;
			m_lastTrans = m_trans;
			m_mouseStart = ReturnMapMousePosition(m_mouseIntPos[0], m_mouseIntPos[1]);
			m_mouseZStart = static_cast<float>(m_mouseIntPos[2]) / static_cast<float>((m_wndWidth-1)/4);
		}
	} else {
		if(m_isClicked)
		{
			MousePosition(m_mouseIntPos[0], m_mouseIntPos[1]);
			m_curRot = m_quat.getMatrix33();
			m_curRot *= m_lastRot;			
			createMatrix44(m_transform, m_curRot, m_trans);

		} else if(m_isRClicked) {
			m_mouseEnd = ReturnMapMousePosition(m_mouseIntPos[0], m_mouseIntPos[1]);
			m_mouseZEnd = static_cast<float>(m_mouseIntPos[2]) / static_cast<float>((m_wndWidth-1)/4);
			m_trans = m_mouseEnd - m_mouseStart;
			m_trans.z = m_mouseZEnd - m_mouseZStart;
			m_trans += m_lastTrans;
			createMatrix44(m_transform, m_curRot, m_trans);			
		} else {
			m_isDragging = false;
			m_mouseIntPos[2] = 0;
			ShowCursor(TRUE);			
			m_isCursor = true;
			m_aInvert = false;
			m_lastRot = m_curRot;
			return;
		}
		
		if(m_isCursor) {
			ShowCursor(FALSE);
			m_isCursor = false;
		}

	}
}

void ArcBall::ApplyCamera()
{
	glLoadIdentity();
	glMultMatrixf(reinterpret_cast<GLfloat*>( &m_transform ));
	glScalef(m_scale, m_scale, m_scale);
}

void ArcBall::ResetCamera()
{
	m_startVec.set(0.0f, 0.0f, 0.0f);
	m_endVec.set(0.0f, 0.0f, 0.0f);
	m_mouseEnd.set(0.0f, 0.0f);
	m_mouseStart.set(0.0f, 0.0f);
	m_mouseZEnd = m_mouseZStart = 0.0f;
	m_lastTrans.set(0.f, 0.f, -STANDARD_TRANSLATION);
	m_trans.set(0.f, 0.f, -STANDARD_TRANSLATION);
	m_lastRot.identity();
	m_invert.identity();
	m_aInvert = true;
	m_transform = fmath::TranslateMatrix44(m_lastTrans.x, m_lastTrans.y, m_lastTrans.z);
	m_curRot.identity();
}

const fmath::matrix33& ArcBall::InvertRotation()
{
	if(!m_aInvert) {
		//m_invert = m_lastRot.invert();
		m_invert = fmath::TransposeMatrix33(m_lastRot);
		m_aInvert = true;
	}
	return m_invert;
}

void ArcBall::DrawToolAxis(const fmath::vector3& toolPos) const
{
	fmath::vector4 ax;
	fmath::matrix44 tmp;

	tmp = fmath::TransposeMatrix33(m_lastRot);
	glBegin(GL_LINES);
	glColor3f(0.10f, 0.5f, 0.10f);
	glVertex3fv(&toolPos.x);
	ax = .8f * tmp[0] + toolPos;
	glVertex3fv((float*)&ax);

	glColor3f(0.10f, 0.10f, 0.5f);
	glVertex3fv(&toolPos.x);
	ax = .8f * tmp[1] + toolPos;
	glVertex3fv((float*)&ax);

	glColor3f(0.5f, 0.10f, 0.10f);
	glVertex3fv(&toolPos.x);
	ax = .8f * tmp[2] + toolPos;
	glVertex3fv((float*)&ax);
	glEnd();

}

void ArcBall::DrawAxis() const
{
	fmath::vector4 ax;
	fmath::matrix44 tmp;

	glPushMatrix();

	tmp = m_transform;
	glLoadIdentity();
	glTranslatef(-3.0f, -3.0f, -10.f);

	glBegin(GL_LINES);
	glColor3f(1.0f, 0.5f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	ax = .5f * tmp[0];
	glVertex3fv((float*)&ax);

	glColor3f(0.0f, 1.0f, 0.5f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	ax = .5f * tmp[1];
	glVertex3fv((float*)&ax);

	glColor3f(0.5f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	ax = .5f * tmp[2];
	glVertex3fv((float*)&ax);
	
	glEnd();
	glPopMatrix();	
}

void ArcBall::DrawLine() const
{
	glBegin(GL_LINES);
		glColor3f(1.0, 0.0, 1.0);
		glVertex2f(m_mouseStart.x * PROMIEN_PIERSCIENIA, m_mouseStart.y * PROMIEN_PIERSCIENIA);
		glVertex2f(m_mouseEnd.x * PROMIEN_PIERSCIENIA, m_mouseEnd.y * PROMIEN_PIERSCIENIA);
	glEnd();
}

void ArcBall::DrawCircle() const
{
	fmath::vector2 lastP;
	float ang = 0, step;

	step = 2*M_PI / IL_FRAGMENTOW_KOLA;
	lastP.set(0.0f, PROMIEN_PIERSCIENIA);
	glBegin(GL_LINES);
		glColor3f(1.0, 1.0, 0.0);
		for(int i=1; i<IL_FRAGMENTOW_KOLA; ++i) {
			ang += step;
			glVertex2f(lastP.x, lastP.y);
			lastP.x = sinf(ang) * PROMIEN_PIERSCIENIA;
			lastP.y = cosf(ang) * PROMIEN_PIERSCIENIA;
			glVertex2f(lastP.x, lastP.y);
		}
		glVertex2f(lastP.x, lastP.y);
		glVertex2f(0.0f, PROMIEN_PIERSCIENIA);

	glEnd();
	glPointSize(5.0f);
	glBegin(GL_POINTS);
	glVertex2f(m_mouseEnd.x * PROMIEN_PIERSCIENIA, m_mouseEnd.y * PROMIEN_PIERSCIENIA);
	glEnd();
	drawPlanes();
}

void ArcBall::bernstein(float u, const fmath::vector2* points, fmath::vector2& bp) const
{
	bp = points[0] * u*u*u;
	bp += points[1] * 3*u*u*(1.0f-u);
	bp += points[2] * 3*u*(1.0f-u)*(1.0f-u);
	bp += points[3] * (1.0f-u)*(1.0f-u)*(1.0f-u);
}

void ArcBall::createMiniPlanes(fmath::matrix44& p1, fmath::matrix44& p2, fmath::matrix44& p3) const
{
	fmath::vector4 ax;
	ax = (m_transform[0] + m_transform[2])/2.0;
	p1.col[0] = -ax;
	p1.col[1] = p1.col[0] + m_transform[0];
	p1.col[2] = p1.col[1] + m_transform[2];
	p1.col[3] = p1.col[0] + m_transform[2];
	p1.col[0].w = p1.col[1].w = 
		p1.col[2].w = p1.col[3].w = 1.0;

	ax = (m_transform[1] + m_transform[2])/2.0;
	p2.col[0] = -ax;
	p2.col[1] = p2.col[0] + m_transform[1];
	p2.col[2] = p2.col[1] + m_transform[2];
	p2.col[3] = p2.col[0] + m_transform[2];
	p2.col[0].w = p2.col[1].w = 
		p2.col[2].w = p2.col[3].w = 1.0;

	ax = (m_transform[1] + m_transform[0])/2.0;
	p3.col[0] = -ax;
	p3.col[1] = p3.col[0] + m_transform[1];
	p3.col[2] = p3.col[1] + m_transform[0];
	p3.col[3] = p3.col[0] + m_transform[0];
	p3.col[0].w = p3.col[1].w = 
		p3.col[2].w = p3.col[3].w = 1.0;

}

void ArcBall::drawPlanes() const
{
	fmath::vector2 off = m_mouseEnd * PROMIEN_PIERSCIENIA;
	fmath::matrix44 p1, p2, p3;
	fmath::matrix44 orto = fmath::PerspectiveMatrix44(
		45.0f, 1.0f, 1.0f, 100.0f);
	//(GLfloat) m_wndHeight/(GLfloat) m_wndWidth

	createMiniPlanes(p1, p2, p3);
	
	p1 = (orto * p1);
	p2 = (orto * p2);
	p3 = (orto * p3);
	p1 *= PROMIEN_PIERSCIENIA/16.0;
	p2 *= PROMIEN_PIERSCIENIA/16.0;
	p3 *= PROMIEN_PIERSCIENIA/16.0;

	glBegin(GL_LINES);
	glColor4f(1.0, 0.0, 0.0, 1.0);
	glVertex2f(p1[0].x + off.x, p1[0].y + off.y);
	glVertex2f(p1[1].x + off.x, p1[1].y + off.y);
	glVertex2f(p1[1].x + off.x, p1[1].y + off.y);
	glVertex2f(p1[2].x + off.x, p1[2].y + off.y);
	glVertex2f(p1[2].x + off.x, p1[2].y + off.y);
	glVertex2f(p1[3].x + off.x, p1[3].y + off.y);
	glVertex2f(p1[3].x + off.x, p1[3].y + off.y);
	glVertex2f(p1[0].x + off.x, p1[0].y + off.y);

	glColor4f(0.0, 1.0, 0.0, 1.0);
	glVertex2f(p3[0].x + off.x, p3[0].y + off.y);
	glVertex2f(p3[1].x + off.x, p3[1].y + off.y);
	glVertex2f(p3[1].x + off.x, p3[1].y + off.y);
	glVertex2f(p3[2].x + off.x, p3[2].y + off.y);
	glVertex2f(p3[2].x + off.x, p3[2].y + off.y);
	glVertex2f(p3[3].x + off.x, p3[3].y + off.y);
	glVertex2f(p3[3].x + off.x, p3[3].y + off.y);
	glVertex2f(p3[0].x + off.x, p3[0].y + off.y);

	glColor4f(0.0, 0.0, 1.0, 1.0);
	glVertex2f(p2[0].x + off.x, p2[0].y + off.y);
	glVertex2f(p2[1].x + off.x, p2[1].y + off.y);
	glVertex2f(p2[1].x + off.x, p2[1].y + off.y);
	glVertex2f(p2[2].x + off.x, p2[2].y + off.y);
	glVertex2f(p2[2].x + off.x, p2[2].y + off.y);
	glVertex2f(p2[3].x + off.x, p2[3].y + off.y);
	glVertex2f(p2[3].x + off.x, p2[3].y + off.y);
	glVertex2f(p2[0].x + off.x, p2[0].y + off.y);
	glEnd();
}

void ArcBall::drawArc() const
{	
	fmath::vector2 ctrlPoints[4];
	ctrlPoints[0] = m_mouseEnd;
	ctrlPoints[0].normalize() *= PROMIEN_PIERSCIENIA;
	ctrlPoints[3] = m_mouseEnd * PROMIEN_PIERSCIENIA;
	ctrlPoints[1].x = (ctrlPoints[3].x + ctrlPoints[0].x)/2.0;
	ctrlPoints[1].y = ctrlPoints[0].y;//(ctrlPoints[3].y + ctrlPoints[0].y)/2.0;
	ctrlPoints[2].x = ctrlPoints[1].x;//(ctrlPoints[3].x + ctrlPoints[0].x)/2.0;
	ctrlPoints[2].y = ctrlPoints[1].y;//(ctrlPoints[3].y + ctrlPoints[0].y)/2.0;

	fmath::vector2 p;
	glBegin(GL_LINE_STRIP);
	for(int i=0; i<=30; i++)
	{
		bernstein(i/30.0f, ctrlPoints, p);
		glVertex2fv(&p.x);
	}
	glEnd();

	glPointSize(5.0);
	glColor4f(0.3, 0.2, 0.5, 1.0);
	glBegin(GL_POINTS);
	for(int j=0; j<4; ++j) {
		glVertex2fv(&ctrlPoints[j].x);
	}
	glEnd();
}

void ArcBall::Draw() const
{
	if(m_isClicked)
		DrawCircle();
	else if(m_isRClicked)
		DrawLine();
}

void ArcBall::AdjustCamToBox(const Box& a_bbox)
{
	float cx, cy, cz;
	cx = a_bbox.m_max[0] - a_bbox.m_min[0];
	cy = a_bbox.m_max[1] - a_bbox.m_min[1];
	cz = a_bbox.m_max[2] - a_bbox.m_min[2];
	cz = max(cx, cz);
	cz = max(cy, cz);
	m_scale = 6.0f / cz;

	m_centering.x = -m_scale * (a_bbox.m_max[0] + a_bbox.m_min[0]) / 2.0f;
	m_centering.y = -m_scale * (a_bbox.m_max[1] + a_bbox.m_min[1]) / 2.0f;
	m_centering.z = -m_scale * (a_bbox.m_max[2] + a_bbox.m_min[2]) / 2.0f;

	createMatrix44(m_transform, m_lastRot, m_trans);
	ApplyCamera();
}